#include "car.hpp"
#include "car_init.hpp"
#include "car_control.hpp"

#include <iostream>
#include <thread>
#include <chrono>

#include "hwt101.hpp"
#include "control_node.hpp"
#include "pid.hpp"

#include <glog/logging.h>

#include "car_step_helper.hpp"

int main()
{
    if (car_init() != YJ_OK)
    {
        while (1)
        {
            LOG(ERROR) << "car init failed";
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }
    }

    if (car_control_init() != YJ_OK)
    {
        while (1)
        {
            LOG(ERROR) << "car init failed";
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }
    }

    int code_color[6] = {1, 2, 3, 3, 2, 1};
    int cjg_color[3] = {3, 2, 1};
    int zc_color[3] = {3, 2, 1};
    Pcar->set_speed(0.0f, 0.0f, 0.0f, 0);
    LOG(INFO) << "car stop";
    std::this_thread::sleep_for(std::chrono::milliseconds(3000));

    std::thread t1(yaw_sensor_read_thread_entry);
    std::thread t2(xy_sensorless_read_thread_entry);
    std::thread t3(car_yaw_control_thread_entry);
    std::thread t4(car_all_control_thread_entry);
    std::thread t5(car_x_control_thread_entry);
    std::thread t6(car_y_control_thread_entry);
    std::thread t7(car_y_claw_control_thread_entry);
    set_car_x_speed_limit(500, -500);
    set_car_y_speed_limit(500, -500);
    set_claw_y_speed_limit(5000, -5000);

    LOG(INFO) << "car inited";
    // reset_mission();
    LOG(INFO) << "reset ok";
    // return 0;
    // while(1){
    // set_car_move(0,200,0);
    // LOG(INFO) << "正";
    // std::this_thread::sleep_for(std::chrono::milliseconds(3000));
    // set_car_move(0,0,0);
    // LOG(INFO) << "回";
    // std::this_thread::sleep_for(std::chrono::milliseconds(3000));}
    int round = 0;
    while(1)
    {
        int cjg_color[3] = {0};
        int zc_color[3] = {0};

        uint8_t temp;

        while(Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_jgq0, temp) !=YJ_OK)
        {
            LOG(ERROR) << "get color not set";
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }


        Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_jgq0, temp);
        cjg_color[0] = temp;
        Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_jgq1, temp);
        cjg_color[1] = temp;
        Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_jgq2, temp);
        cjg_color[2] = temp;
        Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_zcq0, temp);
        zc_color[0] = temp;
        Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_zcq1, temp);
        zc_color[1] = temp;
        Pcar->get_set_color_sq_screen(tjc_screen::tjc_set_color_zcq2, temp);
        zc_color[2] = temp;


        LOG(INFO) << "cjg_color" << cjg_color[0] << cjg_color[1] << cjg_color[2];
        LOG(INFO) << "zc_color" << zc_color[0] << zc_color[1] << zc_color[2];
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        
    }



    // get_ground_color(ground_color);

    // LOG(INFO) << "粗加工区颜色" << ground_color[0] << ground_color[1] << ground_color[2];

    // for (int k = 0; k < 3; k++)
    //     {
    //         if (code_color[k + 3 * round] == zc_color[1])
    //         {
    //             stand_put(1, k, code_color[k + 3 * round], round);
    //         }
    //         else if (code_color[k + 3 * round] == zc_color[0])
    //         {
    //             stand_put(0, k, code_color[k + 3 * round], round);
    //         }
    //         else
    //         {
    //             stand_put(2, k, code_color[k + 3 * round], round);
    //         }
    //     }

    // Pcar->set_rotate_pos(-180,100,240,zdt_motor::motor_pos_mode_absolute);
    // Pcar->set_bracket_pos(BRACKET_TABLE_RANEG, 500, 200, zdt_motor::motor_pos_mode_absolute,true); // 机械臂初始化
    // for (int i = 0; i < 3; i++)
    // {

    //     put_obj(i, 0, -110, -180);
    //     pick_obj(i, 0);
    // }

    // set_car_x_speed_limit(0, 0);
    // double r_angle, claw_y;

    // Pcar->set_claw_channel(1, 70);

    // Pcar->set_bracket_pos(BRACKET_TABLE_RANEG + 50, 500, 200, zdt_motor::motor_pos_mode_absolute);
    // Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute, true);

    // // Pcar->set_lift_pos(LIFT_TABLE_HEIGHT, 100, 200, zdt_motor::motor_pos_mode_absolute);
    // // while (1)
    // // {
    // //     LOG(INFO) << "cam detected move : " << cam->is_move(1);
    // // }

    // while (1)
    // {
    //     Pcar->set_claw_channel(1, 70);
    //     uint64_t start_time = 0;
    //     uint64_t step_time = 0;
    //     for (int j = 0; j < 3; j++)
    //     {

    //         Pcar->set_bracket_pos(BRACKET_TABLE_RANEG + 50, 500, 200, zdt_motor::motor_pos_mode_absolute);
    //         Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute, true);

    //         while (cam->is_move(j + 1) != YJ_OK)
    //             ;
    //         uint64_t time_temp;
    //         time_temp = yj_time_get_millitimestamp();

    //         if (start_trace_color_with_claw((camera::camera_color_en)(j + 1), 20000, 10, true) != YJ_OK)
    //         {
    //             continue;
    //         }
    //         step_time = yj_time_get_millitimestamp() - time_temp;
    //         LOG(INFO) << " pick time: " << step_time;

    //         pick_obj(j, 1);
    //     }
    // }

    return 0;

    // while (1)
    // {
    //     Pcar->set_claw_channel(1, 70);
    //     uint64_t start_time = 0;
    //     uint64_t step_time = 0;
    //     for (int j = 0; j < 3; j++)
    //     {

    //         Pcar->set_bracket_pos(BRACKET_TABLE_RANEG + 50, 500, 200, zdt_motor::motor_pos_mode_absolute);
    //         Pcar->set_rotate_pos(ROTATE_RIGHT_ANGLE, 100, 200, zdt_motor::motor_pos_mode_absolute, true);

    //         if (j == 0)
    //         {
    //             start_trace_color_with_claw((camera::camera_color_en)(j + 1), 120000, 10, true);
    //             while (step_time < 1100)
    //             {
    //                 start_time = yj_time_get_millitimestamp();
    //                 start_trace_color_with_claw((camera::camera_color_en)(j + 1), 120000, 10, true);
    //                 step_time = yj_time_get_millitimestamp() - start_time;
    //             }
    //             start_time = yj_time_get_millitimestamp();
    //             pick_obj(j, 1);
    //         }
    //         else
    //         {
    //             start_time = yj_time_get_millitimestamp();
    //             while (start_trace_color_with_claw((camera::camera_color_en)(j + 1), 20000, 10, true) != YJ_OK)
    //             {
    //                 continue;
    //             }
    //             step_time = yj_time_get_millitimestamp() - start_time;
    //             LOG(INFO) << "2ed 3rd first pick time: " << step_time;

    //             // if (step_time > 20)
    //             // {
    //                 step_time = 0;
    //                 while (step_time < 1100)
    //                 {
    //                     uint64_t time_temp;
    //                     time_temp = yj_time_get_millitimestamp();

    //                     if (start_trace_color_with_claw((camera::camera_color_en)(j + 1), 20000, 10, true) != YJ_OK)
    //                     {
    //                         continue;
    //                     }
    //                     step_time = yj_time_get_millitimestamp() - time_temp;
    //                     LOG(INFO) << " pick time: " << step_time;
    //                 }

    //                 start_time = yj_time_get_millitimestamp();
    //             // }

    //             pick_obj(j, 1);
    //         }
    //     }

    //     return 0;
    // }

    // return 0;
    // while(1){
    // Pcar->set_claw_channel(1, 60); // 释放物料
    // std::this_thread::sleep_for(std::chrono::milliseconds(1500));
    // Pcar->set_claw_channel(1, 75); // 释放物料
    // std::this_thread::sleep_for(std::chrono::milliseconds(1500));
    // Pcar->reset_bracket_pos();                                                                // 机械臂回零
    // Pcar->set_bracket_pos(BRACKET_TABLE_RANEG, 500, 200, zdt_motor::motor_pos_mode_absolute); // 机械臂初始化
    // std::this_thread::sleep_for(std::chrono::milliseconds(1500));
    // }

    // double speed,pos;

    // Pcar->read_bracket_speed_and_pos(speed,pos);
    // LOG(INFO) << "speed: " << speed << " pos: " << pos;
    // Pcar->set_bracket_pos(100,100,100,zdt_motor::motor_pos_mode_absolute);
    // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    // Pcar->read_bracket_speed_and_pos(speed,pos);
    // LOG(INFO) << "speed: " << speed << " pos: " << pos;

    // set_car_x_speed_limit(100, -100);
    // x_pid_in->set_pid_value(0.4f, 0.05f, 0.4f);
    // while (1)
    // {
    //     for (int j = 1; j < 4; j++)
    //     {
    //         Pcar->set_bracket_pos(BRACKET_TABLE_RANEG + 130, 500, 200, zdt_motor::motor_pos_mode_absolute);
    //         start_trace_color(trace_mode_claw_y, (camera::camera_color_en)j, 10);
    //         set_car_x_speed_limit(0, 0);
    //         pick_obj(j - 1, 1);
    //         set_car_x_speed_limit(100, -100);
    //     }
    //     //         1.5f,    // Kp
    //     // 0.02f,   // Ki
    //     // 0.5f,    // Kd
    // }

    // x_pid_in->set_pid_value(0.4f, 0.05f, 0.4f);

    // while(1)
    // {
    // // set_car_move(0.0f, 0.0f, 0.0f);
    // // // waiting_for_move_finish(waiting_mode_car_x, 5000, 10);
    // // set_car_move(100.0f, 0.0f, 0.0f);
    // // waiting_for_move_finish(waiting_mode_car_x, 5000, 10);
    // }

    // reset_arm();
    // double angle, claw_y;

    // while(1)
    // {
    // start_trace_color_with_claw(camera::camera_color_blue, 500000, 5);
    // get_claw_trace_data(angle, claw_y);
    // LOG(INFO) << "angle: " << angle << " claw_y: " << claw_y;
    // put_obj(0, 0, claw_y, angle);
    // std::this_thread::sleep_for(std::chrono::milliseconds(3000));
    // }

    // while (Pcar->read_start_flag() == false)
    // {
    //     std::this_thread::sleep_for(std::chrono::milliseconds(500));
    // }

    // while (1)
    // {
    //     Pcar->scan_code();

    //     uint8_t times, round, color;
    //     for (round = 0; round < 2; round++)
    //     {
    //         for (times = 0; times < 3; times++)
    //         {
    //             if (Pcar->read_color_index_with_round(round, times, color) == YJ_OK)
    //             {
    //                 LOG(INFO) << " round:" << (int)round << "times:" << (int)times << " color:" << (int)color;
    //             }
    //         }
    //     }

    //     std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    // }

    while (1)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
}
